import os
import sys

# Get application directory of Main.py, use that to find the data files.
if hasattr(sys, 'frozen') and sys.frozen:
    appdir, exe = os.path.split(sys.executable)
else:
    dirname = os.path.dirname(sys.argv[0])
    if dirname and dirname != os.curdir:
        appdir = dirname
    else:
        appdir = os.getcwd()

M_DIR = os.path.join(appdir, 'Matlab Data')
IM_FILENAME = os.path.join(M_DIR, "datfile2.DAT")
MOV_FILENAME = os.path.join(M_DIR, "movfile2.DAT")

# NOTE: A duplicate of the first frame is needed for successfull simulation, because the Sensors panel needs a frame to check
# the position of the sensors, otherwise the pointer of the parser reaches an EOF.

## This class simulates the FoB driver using exisiting calibration and motion files.

# After finishing the calibration steps in the program, 
# change the mode to movement mode by setting f.mode = 1 where f is an instance of the fake_fob.FOB class.
class FOB:
    ##  Constructs a simulator class for the FoBDriver class.
    def __init__(self):
        ##  Serial() instance.
        self.ser = "FakeFOB SER queried"
        ##  Dat file.
        self.im_file = file(IM_FILENAME)
        ##  Mov file.
        self.mov_file = file(MOV_FILENAME)
        ## Calibration mode = 0. Movement mode = 1.
        self.mode = 0

    ##  Pass.
    def open(self, p1, p2, p3):
        pass

    ##  Pass.
    def fbbAutoConfig(self, numbirds):
        pass

    ##  Pass.
    def fbbReset(self):
        pass

    ##  Pass.
    def position_matrix(self, idx):
        pass

    ##  Pass.
    def run(self):
        pass

    ##  Pass.
    def sleep(self):
        pass

    ##  Pass.
    def close(self):
        pass

    ##  This returns a 12-element list: first 3 are the position, next 9 represent a 3x3 transformation matrix.
    # During the calibration phase, we only return the data belonging to sensor 2.
    # During the movement phase we return any data that is requested (we know which indexes to return).
    def point(self, idx):
        # Keep on reading lines until we reach a line with 4 elements, the first element is the sensor index.
        # Keep on reading until we have a 4 x 3 matrix.
        cur_sidx = -1   # Current sensor idx
        cur_row = 0     # Current row in 4 x 3 matrix.
        m = [0] * 12

        if self.mode == 0:
            for l in self.im_file:
                elems = [float(i) for i in l.split()]
                # Eerste rij van matrix aangetroffen.
                if len(elems) == 4:
                    cur_sidx = int(elems[0])
                    # Opslitsen en toevoegen aan onze 3x4 matrix.
                    m[0:3] = elems[1:4]
                    cur_row = 1
                # Alleen als de sensor ook de opgevraagde sensor is, dan gaan we de rotatie ook toevoegen.
                elif len(elems) == 3 and cur_sidx == idx:
                    blat = cur_row * 3
                    m[blat:blat+3] = elems[0:3]
                    cur_row += 1
                # cur_row wordt nooit 4 als we niet met de opgevraagde sensor te maken hebben.
                if cur_row == 4:
                    break

        else:
            for l in self.mov_file:
                elems = [float(i) for i in l.split()]
                if len(elems) == 4:
                    cur_sidx = int(elems[0])
                    m[0:3] = elems[1:4]
                    cur_row = 1
                elif len(elems) == 3 and cur_sidx == idx:
                    blat = cur_row * 3
                    m[blat:blat+3] = elems[0:3]
                    cur_row += 1

                if cur_row == 4:
                    break

        if cur_row != 4:
            if self.mode == 0:
                raise RuntimeError, "Reached end of file while parsing"

            else:
                self.mov_file.seek(0)
                return self.point(idx)
        else:
            return m